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Abstract 

The  motivation  for  this  work  is  the  real-time  solution  of  a  standard  optimal  control  problem 
arising  in  robotics  and  aerospace  applications.  For  example,  the  trajectory  planning  problem 
for  air  vehicles  is  naturally  cast  as  an  optimal  control  problem  on  the  tangent  bundle  of  the  Lie 
Group  SE( 3),  which  is  also  a  parallelizable  Riemannian  manifold.  For  an  optimal  control  prob¬ 
lem  on  the  tangent  bundle  of  such  a  manifold,  we  use  frame  co-ordinates  and  obtain  first-order 
necessary  conditions  employing  calculus  of  variations.  The  use  of  frame  co-ordinates  means  that 
intrinsic  quantities  like  the  Levi-Civita  connection  and  Riemannian  curvature  tensor  appear  in 
the  equations  for  the  co-states.  The  resulting  equations  are  singularity-free  and  considerably 
simpler  (from  a  numerical  perspective)  than  those  obtained  using  a  local  co-ordinates  repre¬ 
sentation,  and  are  thus  better  from  a  computational  point  of  view.  The  first  order  necessary 
conditions  result  in  a  two  point  boundary  value  problem  which  we  successfully  solve  by  means 
of  a  Modified  Simple  Shooting  Method. 
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1  Introduction 


This  paper  studies  regular  solutions  of  optimal  control  problems  for  simple  mechanical  systems 
both  from  a  theoretical  and  a  computational  point  of  view.  The  motivation  for  this  paper  is  the 
real-time  trajectory  planning  problem  for  hypersonic  aircraft.  The  usual  separation  of  time-scales 
assumption  that  is  made  for  air-vehicles  fails  for  such  aircraft.  Therefore  the  trajectory  planning 
must  be  done  simultaneously  for  both  the  attitude  and  position  variables.  An  air-vehicle  can  be 
thought  of  as  evolving  on  the  tangent  bundle  of  the  Lie  Group  SE( 3)  with  a  Riemannian  metric 
that  is  obtained  from  the  total  kinetic  energy.  We  assume  that  the  center-of-mass  and  principal 
moments-of-inertias  do  not  change  during  the  flight,  and  consider  the  aerodynamic  forces  and 
moments  to  be  input  variables. 

Jurdejevic  [1]  and  Krishnaprasad  [2]  have  considered  optimal  control  problems  for  left-invariant 
systems  on  Lie  Groups,  with  the  input  variables  affecting  the  velocity  vector  field  on  the  configu¬ 
ration  space.  Simple  mechanical  systems  that  are  the  focus  of  this  paper,  with  forces  and  moments 
as  inputs,  do  not  satisfy  this  framework.  Sussmann  [3]  tackled  the  problem  of  generalizing  the 
Pontryagin’s  Minimum  Principle  to  manifolds  (without  any  affine-connection  structure),  by  devel¬ 
oping  the  co-ordinate  free  maximum  principle.  For  computational  purposes,  when  this  principle  is 
applied  to  an  air-vehicle  problem,  one  employs  local  co-ordinates  and  the  equations  reduce  to  the 
necessary  conditions  for  an  optimal  control  problem  in  co-ordinates.  Local  co-ordinates  might  not 
be  the  best  choice  possible  for  the  real-time  computation  of  the  optimal  trajectory  when  one  has 
an  additional  Lie  Group  structure.  This  is  because  a  suitable  choice  of  co-ordinates  depends  on  the 
initial  and  final  conditions  on  the  optimal  control  problem,  which  makes  it  unsuitable  for  real-time 
computation  of  optimal  trajectories.  If  the  configuration  space  is  a  connected  Lie  Group  G  with 
Lie  Algebra  Q,  then  one  can  represent  any  point  as  a  product  of  exponentials  using  the  exponential 
map,  exp  :  Q  — >  G.  In  general,  this  map  is  not  globally  one-to-one  or  onto,  but  in  the  case  of  groups 
that  semi-direct  products  of  a  connected  and  compact  group  and  a  connected  Abelian  group,  it 
is  globally  one-to-one  and  onto  (except  on  a  set  of  measure  zero).  Such  groups  arise  naturally  in 
robotics  and  simple  mechanical  systems.  The  first  order  necessary  conditions  obtained  in  this  paper 
using  frame  co-ordinates  on  the  tangent  bundle  are  especially  suited  for  real-time  computation  in 
such  applications.  We  obtain  our  results  for  a  parallelizable  Riemannian  manifold,  though  our  main 
interest  lies  in  simple  mechanical  systems  on  Lie  Groups.  Thus  the  method  is  frame  dependent 
rather  than  invariant.  This  approach  can  also  be  seen  in  the  work  of  P.  Crouch,  M.  Camarinha 
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and  F.  Silva  Leite  [4,  5,  6].  Crouch  et  al.  consider  a  special  case  of  the  problem  investigated  in 
this  paper.  Their  work  and  ours  differ  in  the  nature  of  variations  considered,  and  therefore  the 
necessary  conditions  obtained  in  this  paper  are  different  from  those  of  Crouch  et  al. 

The  main  motivation  for  the  choice  of  frame  co-ordinates,  is  that  the  resulting  first-order  nec¬ 
essary  conditions  can  be  solved  using  a  numerical  method  called  the  Modified  Simple  Shooting 
Method  [7].  Though  numerical  methods  for  optimal  control  problems  on  local  co-ordinates  have  a 
long  history,  there  is  a  dearth  of  literature  on  numerical  methods  using  frame  co-ordinates.  This 
paper,  we  believe,  is  the  first  to  address  this  issue.  Usually  optimal  control  problems  in  local  co¬ 
ordinates  are  tackled  either  by  a  direct  method  [8,  9]  using  non-linear  programming  methods,  or  by 
an  indirect  method  that  uses  Pontryagin’s  Minimum  Principle,  resulting  in  a  two-point  boundary 
value  problem  [10,  11].  In  this  paper,  we  use  the  indirect  approach  to  the  solution  of  an  optimal 
control  problem.  Among  the  indirect  methods,  the  Modified  Simple  Shooting  Method  (MSSM)  has 
been  shown  to  be  more  accurate  and  resulting  in  faster  computation  times  than  other  methods 
such  as  the  Multiple  Shooting  (MSM),  Finite  Difference  and  Collocation  methods  [7].  In  related 
research,  it  was  found  that  the  earlier  MSM  failed  to  converge  numerically  for  an  optimal  control 
problem  on  TSO( 3)  while  using  frame  co-ordinates,  while  the  MSSM  converged  successfully  [12]. 
In  this  paper,  we  consider  the  more  complex  problem  of  a  rigid  body  and  show  that  the  MSSM 
converges  successfully  in  this  case  also. 

2  Optimal  Control  on  Parallelizable  Riemannian  Manifolds 

In  this  section,  we  derive  necessary  conditions  for  regular  solutions  of  optimal  control  problems 
on  parallelizable  Riemannian  manifolds.  Previous  work  in  this  area  was  done  by  P.  Crouch,  M. 
Camarinha  and  F.  Silva  Leite  in  a  series  of  papers  [4,  5,  6].  They  considered  a  special  case  of  the 
problem  considered  here  and  use  a  different  approach  to  obtain  first-order  necessary  conditions. 

2.1  Parallelizable  manifolds  and  Cartan  Formalism 

The  mathematical  background  in  Riemannian  geometry  used  in  this  paper  can  be  found  in  standard 
sources  such  as  Frankel  [14]  or  Boothby  [15].  If  there  exists  a  set  of  n  smooth  vector  fields  on  a 
manifold  M  of  dimension  n  that  are  linearly  independent  at  each  point,  then  the  manifold  M  is  said 
to  be  parallelizable  [15].  Such  a  set  of  n  linearly  independent  vector  fields  is  referred  to  as  a  field  of 
co-ordinate  frames  or  briefly  as  a  frame.  It  is  well  known  that  the  condition  of  being  parallelizable 


3 


is  very  special  for  a  manifold.  For  example,  among  the  spheres  Sn,  n  =  1,2,---  ,  only  S1,  S 3  and 
S 7  are  parallelizable.  However,  it  is  well  known  that  all  Lie  Groups  are  par alleliz able  [15].  In  the 
case  of  air  vehicles,  the  configuration  space  is  the  Lie  Group  SE(3).  However  the  manifold  on  which 
the  air  vehicle  dynamics  can  be  described  is  TSE( 3)  -  the  tangent  bundle  of  SE(3).  The  following 
lemma  states  that  TSE( 3)  is  parallelizable.  The  proof  follows  from  the  observation  that  locally 
TM  is  a  product  manifold  U  x  IR”  where  U  is  a  co-ordinate  neighborhood  of  M. 

Lemma  2.1  Let  M  be  a  parallelizable  manifold.  Then  TM  is  parallelizable. 

Let  M  be  a  manifold  with  a  symmetric,  positive  definite,  and  bilinear  form  (called  a  Riemannian 
metric)  defined  on  TqM  where  q  £  M,  denoted  by  <  •,  •  >q  .  We  assume  it  to  be  smooth  for  each 
q  £  M  and  C 1  as  a  function  of  q.  A  Riemannian  metric  defines  a  linear  isomorphism:  E  :  TqM  — > 
T*M  (where  q  £  M  and  T*M  is  the  dual  of  TqM )  by:  (E(u))  (w):=  <  v,w  >q\  v,w  £  TqM.  As 
E  is  an  isomorphism,  we  also  have  the  inverse  map  E-1  :  T*M  — >  TqM  where  q  £  M. 

Assume  M  to  be  parallelizable  and  consider  a  frame  {Ei}  and  a  dual  co-frame  {0*},  i  =  1,2 ,  •  •  •  n 
with  6l(Ej)  =  Sj.  Then  the  Levi-Civita  connection  is  defined  by  the  functions  in  the  equation: 

V Et Ej  =  uj^j Ey. ,  1  <  i,j  <  n.  If  we  denote  u)ky.=io^  9\  then  we  can  define  the  n  x  n  matrix  tv  of 

connection  one- forms  by  tv  =  (w^),  where  k  indicates  the  row  index  and  j  the  column  index.  The 
following  proposition  is  useful  in  the  computation  of  oj  : 

Proposition  2.1  [15]  Let  <  Ei,Ej  >=  rriij .  Define  d6  =  (dO1,---  ,d9n).  Then  there  is  a  unique 
matrix  of  1-forms  u  =  (ujL  ^  such  that:  dd  =  —tv  AO,  that  is,  dOl  =  —a ;L  A#7  and  ujrimrj+ujrjmri  = 
0;  i,j  =  !,■■■  ,n. 

The  computation  of  d0l  is  done  using  Theorem  4.25  of  Frankel  [14].  Cartan  showed  that  the 
curvature  tensor  R(-,  •),  •  can  be  computed  from  the  connection  matrix  as  follows: 

Proposition  2.2  [14]  The  equations  Q  =  dw  +  wAw,  that  is,  TT-  =  doj'j  +ujlkAujh '■  define  a  skew- 
symmetric  matrix  of  2-forms  that  is  related  to  the  curvature  tensor  via  R(X,  Y)Ej  =  HA  (A,  Y)  ■ Ei . 


2.2  Derivation  of  first-order  necessary  conditions 

Let  M  be  a  parallelizable  Riemannian  manifold.  If  c  :  [0, 1]  — ►  M  is  a  differentiable  curve  on  M,  and 
X  :  M  — ►  TM  is  a  differentiable  vector  field,  then  the  co- variant  derivative  of  X  along  c(-)  is  defined 
to  be  =  Vc(t)X(t),  t  £  (0, 1).  Let  {£1,  •  •  •  ,  En}  be  a  frame  of  vector  fields  and  let  {0l,  •  •  •  ,  6n} 
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be  a  frame  of  co-vector  fields  on  M ,  so  that  Ol(Ej)  =  S'y,  1  <  i,j  <  n.  Let  q  £  M;  V  £  TqM  and 
u  £  ]R.m  denote  the  control  variables.  We  define  a  control  system  on  TM  by  a  second-order  vector 
field  F  :  TM  x  IRm  —  TTM  defined  as  follows.  If  n  :  TM  — *•  M  denotes  the  projection  operator, 
then  a  second-order  vector  field  is  one  that  satisfies  dir  o  E^qVju\  =  ( q ,  V).  Using  the  Levi-Civita 
connection  on  M,  we  can  write  the  above  system  as  one  on  TM  described  by  the  equations: 

q=V  =  ViEi,  and  ^  =  f(q,V,u)  =  /'(<?,  V,<z)£*.  (1) 

The  conditions  on  the  function  /  will  be  set  forth  in  our  theorem  on  necessary  conditions.  Such  a 
set  of  equations  is  useful  in  describing  the  equations  of  motion  of  an  air  vehicle  that  is  subject  to 
aerodynamic  forces  and  moments  (as  well  as  gravity)  that  depend  on  its  orientation  with  respect 
to  its  velocity  vector,  its  altitude  above  sea  level,  current  speed  and  the  deflections  of  its  control 
surfaces. 

Now,  let  £  M,  Vo  £  Tq0 M  and  Vf  £  Tq;  M.  Consider  the  space  C2[to,tf]  of  twice- 

differentiable  maps  q  :  [to,tf]  — >  M  that  satisfy  Equations  (1),  where  tf  >  to,  q{to)  =  qo,  <l(tf)  =  qf, 
q(to)  =  Vo  and  q(tf)  =  Vf.  Then  along  one  such  map  q(-)  the  control  system  takes  the  form: 
q(t)  =  V(t),  and  ^  =  f{q{t),V(t),u(t)),  where  u(-)  £  Cm[to,tf],  the  (nr- vector  valued)  space 
of  continuous  functions.  Suppose  that  one  is  required  to  find  a  function  u(-)  such  that  the  above 
boundary  conditions  are  satisfied  by  q(-)  while  minimizing: 

J{u{-))=[  L(q(t),V(t),u(t))dt.  (2) 

Jto 

We  need  the  following  standard  construction  to  describe  the  notion  of  variations  of  a  curve.  Let 
(f,  <t)  — >  q(t,a),  t  £  [to,tf]  and  a  £  (— e,  e),  e>  0,  be  a  parametrized  family  of  curves  satisfying 

q{t,0)  =  q{t);  q{t0,a)  =  q0 ;  q(tf,a)=qf;  q(t0,  a)  =  V0;  q(tf,a)  =  Vf  (3) 

For  V(t)  £  T9(t)M  and  pi{t),p2{t)  £  T*^M,  we  define  the  associated  variations:  V(t,a)  = 
Vl{t,a)Ei{q(t,(j))  £  Tq{Ua)M  andpi(t,cr)  =  Pu(t,  cr)0l(q(t,  a)),  p2(t,  a)  =  p2i{t,  cr)6l{q{t,  cr))  £ 
Tq(t  a) M-  Define  the  variational  vector  fields  W(t)  =  Sq(t)  :=  0)  £  Tq^M,  t.  £  [to,tf],  with 

W(to)  =  W(tf )  =  0;  and  6V(t)  :=^D(t,0)  £  Tq^M,  t  £  [to, £/].  The  variations  in  the  input 
are  denoted  by  u(t,a)  £  !Rm  with  6u(t)  :=|^(t,0)  £  !Rm.  In  the  following,  any  quantity  that 
is  described  with  the  second  variable  a  suppressed,  should  be  construed  as  having  a  =  0  (so, 

q(t)  =  q(t,  0)). 

Then  we  have  the  following  lemma  proved  by  Noakes,  Heinzinger  and  Paden. 
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Lemma  2.2  [13]  ^-(to,cr)  =  0  and  ^r( tf,a )  =  0  for  all  a  £  (— e,  e). 

We  need  the  following  simple  lemmata  in  the  proof  of  the  main  theorem  (Theorem  2.1). 

Lemma  2.3 

/  =  -  (4) 
J  to  J  to 

Proof  We  have  ^(t)  =  ^~(f)  because  the  Levi-Civita  connection  is  symmetric  and  [J^,  JL]  =  0. 
Therefore: 

i'jtMsq)dt  =  s‘  s  dt= r  {<ft^’sq> +<^4t 

The  result  follows  by  noting  that  W(to)  =  W(tf)  =  0.  ■ 

Lemma  2.4 

C  P2{t)^^dt  =  ft  ((SjR(S“1;P2’  V)V)5q  ~  ^5V{t))  )  dt  (5) 

Proof  First  we  note  that:  P2(t)  Jy  ^jfdt  =  P‘i{t)  {R(W,V)V  +  £^L)  dt,  by  the  definition 

of  the  curvature  tensor  [14,  15],  and  the  fact  that  [J^,  Jb]  =  o.  By  the  definition  of  the  linear 
isomorphism  E,  and  standard  properties  of  the  curvature  tensor  [14]  we  have:  p2  (R(W,V)V)  =< 
Z~1p2,  R(W,V)V  >=<  R(Y,-1p2,V)V,W  >=  (Si?(S-1p2,P)l/)(lT).  On  integrating  the  second 
term  by  parts  and  using  Lemma  2.2,  we  have  the  claim.  ■ 

For  p  £  T*M,  denote:  [u(f)]*p  :=  PiWlj  {f)&  =  Vi^kj  fk& ■  Let  Clk-  denote  the  structure  constants 
for  the  Jacobi-Lie  brackets  of  the  coordinate  vector  fields.  Also  denote:  [C(f)]*p:=piClj  (/)0J  = 
PiCij  fk0i .  The  following  theorem  is  the  main  result  of  this  paper.  It  establishes  the  first-order 
necessary  conditions  for  the  curve  (qo,  Vo,  uo)(t),  t  £  [to,tf]  to  be  optimal. 

Theorem  2.1  Suppose  that  (qo,Vo,uo)(t),  t.  £  [to,tf]  minimizes  the  cost  function  (2),  while  sat¬ 
isfying  Equations  (1)  and  boundary  conditions  q(to)  =  %,  q(tf)  =  qf,  q(to)  =  Vq  and  q(tf )  =  Vf. 
Further  suppose  that  f  and  L  are  differentiable  functions  of  their  arguments,  and  the  linearized 
system  is  controllable  at  the  origin.  Then  there  exists  one-forms  pi(t),p2(t)  differentiable  almost 
everywhere  on  [to,tf]  such  that: 

^  =  Lq(q0,V0,u0)  +  ZR(Z-1p2,Vo)Vo-(rq(qo,V0,uo)+u*(f)-C*(f))p2 
yjT  =  ~Pi  +  Lv(qo,V0,uo)  -  fy(qo,Vo,u0)p2 
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fu(Qo,V0,uo)p2  =  Lu(q0,v0,u0 ) 


•  the  function  H(q0,V0,u0,p1,p2){t)  =  L(q0,Vo,u0)(t)  -  pi(V0)(t)  -  P2(f(qo,  V0,  u0))(t)  is  a 
constant  for  t  €  [to,tf]. 

The  proof  of  the  above  theorem  will  be  given  shortly.  The  assumption  of  controllability  of  the 
linearized  system  is  strong,  and  can  be  weakened  along  the  lines  of  Pontryagin’s  maximum  principle 
[3],  once  the  symplectic  2-form  on  T*TM  is  written  in  frame  co-ordinates.  Such  a  symplectic  form 
is  not  intrinsic  and  depends  on  the  Riemannian  metric  on  TM  as  we  show  in  a  future  publication. 
In  any  case,  the  results  of  this  paper  will  serve  as  a  check  for  the  first  order  necessary  conditions 
that  can  be  obtained  using  more  sophisticated  tools  of  symplectic  geometry.  The  proof  here  follows 
Luenberger  [16],  and  uses  the  Lagrange  Multiplier  theorem  to  obtain  the  existence  of  the  one-forms 
Pi(-),P2{')-  Please  refer  to  Luenberger  to  see  how  the  controllability  assumption  enters  the  proof. 
(Proof  of  Theorem  2.1) 

For  each  a  £  (— e,  e),  consider  the  augmented  cost  function: 

j(q(-,v),V(-,a),pi(-,<T),p2(-,cr))  =  ^  (^L{q,V,u)  +pi{q-  V)  +  Pii^f  ~  dt,  (6) 

where  q,  V,p\,p2  in  the  integral  are  functions  of  (t.  a).  By  the  Chain  Rule: 

T^L(q,V,u)(t,  0)  =  Lq(q,V,u)(t,0)(Sq(t))  +  Lv(q,  V,  u)(t,  0)  +  Lu(q,  V,  u)(t,  0)(5u(t)). 

(7) 

T^f(q,V,u)(t,  o)  =  V,u){t,0){5q)  +  fy{q,  V,u)(t,0)(^r(t))  ••• 

+/£(?,  V,  u)(t,  0  Ek(t,  0)  +  fk(q,  V ,  u)(t,  (8) 

Lets  consider  the  last  term  in  the  above  equation. 

fk(q,V,u)(t,0)DEkd^°]  =  /fc(g,  =  fk{q,V,u){t)^ij  -  E,.  (9) 

Now  by  Lebesgue’s  Dominated  Convergence  Theorem,  we  have: 

^(9, !■>„«)(•, 0)  =  £'  (£m«v,u)  +P1(^)  -MW)  +  »^/(?.  dt. 

By  Lemmas  2.3,  2.4  and  Equations  (7-9),  we  get: 

^(q,V,Pl,P2)(;0)  =  jf'  ((Lq(q,V,u)  -  ^  +  ^R(Tt~lp2,V)V  -  (/*  +  [a ;(/)]*  -  [C(f)]*)p2)6q 

+{Lv{q,  V,  u)-P!-  -  ff,P2)5V  +  (. Lu(q ,  V,  u)  -  f*P2)Suj  dt 
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As  the  variations  8q,  5V  and  5u  are  arbitrary,  subject  to  Sq(to)  =  Sq(tf)  =  5V(to)  =  SV(tf)  =  0 
we  have  the  first  two  statements  of  the  theorem. 

To  prove  the  last  statement  of  the  theorem,  consider 


H(t)  =  ^L(q0,Vo,uo)(t)  -  ~^(vo)  ~  Pi(^)(t)  -  ^-(f(q0,V0,u0))(t)  -  p2(£/(go,  Vo,  u0))(t) 

DV 

=  Lq(qQ,VQ,uo)(VQ)  +  Lv(q0,V0,uo)(—)  +  Lu(q0,  V0,  u0)(u) 

-  ( Lq(q0 ,  V0,  u0 )  +  Vo)  Vo  -  (fq  (go,  V0,  uq)  +  u*(f)  -  C*(f))p2 )  (V0)(t) 

~Pi  (f(qo,Vo,u0))  (t)  -  (-pi  +  Lv(q0,V0,uo)  -  fv{qo,V0,u0)p2)  (f(qo,V0,u0))(t) 

-P2  (jq(qo,  Vo,u0)(V0)  +  fv(qo,  V0,u0)(^)  +  fu(qo,Vo,uQ)(u)  + 

=  o, 

due  to  the  fact  that  <R(T,~1p2 ,  Vo)  Vo,  Vo  >=  0  and  fk(q,  V,  u)(t,  0)  DEkj£'°'>  =  fk{q,  V,  u)(t)ujl-kq^ Ei  = 

3  Applications 

3.1  Cubic  splines  on  Riemannian  manifolds 

Here  we  specialize  Theorem  2.1  and  recover  the  Noakes,  Heinzinger  and  Paden  formula  for  cubic 
splines  on  Riemannian  manifolds  [13].  Let  M  be  a  parallelizable  Riemannian  manifold  and  let 
qo,qi  £  M,  Vo  G  TqoM  and  Vi  G  TqiM.  Consider  the  problem:  Minimize  J(u(-))  =  ||u(t)||2df 

subject  to:  q(t)  =  V(t),  pr-  =  u(t)  =  ul(t)Ei(t),  and  boundary  conditions  (/(to)  =  qo ;  (/(t/)  = 

qf,  q(to)  =  Lo;  q(tf)  =  Vf. 

Thus  we  have  f(q,  V,u)  =  u  and  E  is  the  identity  matrix.  Therefore,  there  is  an  identification  of 
vectors  and  co- vectors.  Then,  Theorem  2.1  asserts  the  existence  of  one- form  sections  Pi(t),p2(t) 
such  that:  =  R(p2,V)V  —  ( cv*(f )  —  C*(f))p2 ,  pp  =  —p\  and  p2  =  u,  where  we  have  used 

the  identification  of  vectors  and  co-vectors  in  the  last  equation.  Thus  pp-  =  pp  =  pfp  =  —pi 
which  implies  =  =  -R(P2,V)V  +  (u*(f)  -  C*(f))p2.  Now  ((c S(f)  -  C*(f))p2)w  = 

p2i (iolj  (p2)  -  Clj  (p2))  wj  =  p2i  u>ljk  p\  wj  =  u\ ( w )  pk  p2i  =  0  for  all  w  G  ^ (M) ,  because  lo\  =  -uk 
by  Proposition  2.1.  Therefore,  p^p  +  V)V  =  0,  which  is  the  equation  for  a  cubic  spline  that 

was  first  obtained  by  Noakes,  Heinzinger  and  Paden  [13]. 


3.2  Rigid  body  translation  and  rotation 


In  this  subsection,  we  consider  the  problem  of  numerically  solving  the  optimal  control  problem  (1), 
(2)  and  (3)  for  a  rigid  body  that  is  free  to  rotate  and  translate.  Consider  a  rigid  body  with  principal 
moment  of  inertia  matrix  1  and  mass  M  (a  scalar).  The  configuration  space  of  a  rigid  body  SE( 3) 
is  the  space  of  rotations  given  by  the  set  of  3  x  3  matrices  50(3)  =  {Q  \  Q'Q  =  /;  det(Q)  =  1}, 
and  space  of  translations  1R3.  Q  is  the  orientation  of  the  rigid  body  with  respect  to  an  earth- fixed 
co-ordinate  system,  while  b  is  the  position  of  the  center  of  mass  of  the  rigid  body  with  respect  to  the 
origin  of  the  earth-fixed  co-ordinate  system.  The  angular  velocity  of  the  body  in  the  principal  axis 
system  (called  the  body  axis  system)  centered  at  the  center  of  mass  is  defined  as:  id  =  Q'Q  where 
Q1  denotes  the  transpose  of  Q,  while  the  linear  velocity  of  the  center  of  mass  expressed  in  the  body 
axis  system  is  denoted  by:  v  =  Q'b.  If  we  define  the  skew-symmetrization  of  Hr  =  [Oi ,  ST2 ,  ST3] 


to  be  = 


0 

— H2 

compact  form  as: 


— H3 
0 

Hi 


H2 
— Hi 
0 


then  the  Euler’s  equations  for  a  rigid  body  can  be  written  in  a 


Q 

=  QH 

(10) 

b 

=  Qv 

(11) 

H  -  IT1  (IIH  x  H) 

=  I  ~XTe 

(12) 

i  +  Hx» 

=  M~xFe. 

(13) 

where  Te,  Fe  are  the  moments  and  forces  acting  on  the  body  (expressed  in  the  principal  body- 
axis  system).  In  the  notation  of  Theorem  2.1,  q  =  (Q,b),  V  =  (H,  v)  and  u  =  (H-1Te, M~1Fe). 
Given  (Q,  b ,  H,  u)(0),  (Q,  b ,  H,  u)(l),  and  L(q,  V,  u)  =  \  <  u,  u  >,  the  optimal  control  problem  is  to 
determine  u(t)\  t  G  [0, 1].  We  set  up  a  two-point  boundary  value  problem  using  Theorem  2.1,  and 
then  obtain  a  numerical  solution  using  the  Modified  Simple  Shooting  method  [7] . 

We  will  first  find  the  Levi-Civita  connection  that  corresponds  to  the  Kinetic  Energy  metric: 
K (Q,  6,  Q,  b)  =  —  jtrace(HIH)  +  ^ MbT  b.  Let  {ei,  •  •  •  ,  e^}  form  a  basis  for  the  Lie  algebra  se(3)  = 
so( 3)  ©M3  of  the  Lie  Group  SE( 3),  where  ei,  e-2,  e-z  form  a  basis  for  so{ 3)  and  e^,  e$,  e q  form  a  basis 
for  1R3 .  Then  one  can  form  a  parallel  frame  Ei  =  Qei ,  i  =  1,  ■  ■  •  ,6  for  TSE( 3)  via  the  lift  of  the 
left  translation  action.  For  this  parallel  frame  {E\,  ■  ■  ■  ,  Eq}  let  the  structure  constants  for  the 
Jacobi-Lie  bracket  be  given  by: 
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C32  =  1 

<4  =  1 

cf 1  =  1 

Cf5  =  1 

cl,  =  -1 

Cl 4  =  -1 

<4  = 1 

C'm  =  1 

<4  =  -1 

Pi 

=  0  for  all  other  1  <  i,  j,  k  <  6. 

The  Riemannian  metric  is  defined  via  the  following  table: 


<  E\,  Ei  >=  2/1 

<  E2 ,  A2  >=  ^2 

<  A3,  E;i  >=  2^3 

<  a4,  e4  >=  1 

<  E§,  A5  >=  1 

<  e,,e,  >=  1 

<  Ei,  Ej  >=  0  for  all  other  1  <  i,  j  <  6. 

We  can  compute  the  connection  matrix  for  the  rigid  body  using  Proposition  2.1  and  Theorem  4.25 
of  Frankel  [14].  As  ^(Ej)  =  5),  we  have:  -  E^Q^Ej))  -  P^E^E*])  = 

— 6l([Ej,  Efr]).  Using  Proposition  2.1  we  compute  the  following  connection  matrix: 


r  1 

0 

w32^3 

w23^2 

0 

- e 3 

02 

r  1 

P 

O 

CO 

X 

CO 

= 

,  where  a  = 

wIi  °3 

0 

,P  = 

e3 

0 

-01 

L  J 

0 

CO 

X 

CO 

+\2ol 

1 

1 

to 

6 1 

L  J 

_  u 21 °2 

0 

0 

where  uj12  =  \  (  7l  i[2+Ia 


i l -h+h-h 
2  l  h 


1  _  1  ( h~h+h\ 

23  -  2  V  h  J 


0J31  — 


=  1 ( h+h-h 


2  V  h 


.2  _  1  /  h-h-h 

A3  -  2  l  I2 


,  ,3  _ 

1+21  ~ 


^  ,  and  +12  =  l  (  h+h+h ^  _  Next,  we  compute  the  matrix  V eE  [14]: 


[VEjEi_ 


L  M 

03x3  03x3 


where  L 


0  cuf2A3  to>f3A2 

0 

Ae 

-A5 

w2lA"3  0  CU23A1 

,  M  = 

—E, 

0 

a4 

CU2,P2  cu32Ai  0 

A5 

-a4 

0 

Let  X  =  (Q,v),Y  =  (£,v)  G  \&(SA( 3));  Q,v,£,v  G  1R3  and  q(t)  be  the  curve  obtained  by  solving 
the  system  q(t)  =  X ;  q(to)  =  qo]  t  G  [to,t/]-  Then  along  the  curve  q(-),  we  compute: 


?+  V  C^(E 'j)WEk 

Vn? 

W  = 

i1j1k= 1 

= 

V  —  V  X  Q 

V  —  V  X  Q 

where  V  is  the  Levi-Civita  connection  on  SO(3)  compatible  with  the  Kinetic  energy  metric  K(Q,  Q)  = 

h  - 1-1  (in  x  n) 

v  —  v  x  U 


—  itrace(ni?2).  In  particular,  =  VjA  = 


.  We  compute  the  curvature 


tensor  R[Y,X)X  using  Proposition  2.2.  The  curvature  two-form  matrix  turns  out  to  be: 


r  1 

0 

A#1  A  e2 

B93  A  01 

_  7. 

p 

03x3 

A01  A  6>2 

c#2  a  e3 

— 

0.3x3 

P 

,  where  P  = 

0 

L  -  J 

EO3  A  01 

A#2  A  03 

0 
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and  R  = 


,  where,  A  =  -u\2  -  u;%3uf2;  B  =  ~w23  +  w32wi3> 


0  HO 1  A  0 2  I03  A  01 

AT01  A  02  0  J02  A  6 3 

L63  A  6*1  M 6>2  A  #3  0 

L  —  —  W13  —  W31u;23,  —  — W31  +  W13(J21)  -C'  —  ~UJ21  ~  ^31^12)  P  ~  ~U112  +  c°,21  w32 

H  =  I  =  J  =  I<  =  L  =  M  =  0.  Next,  as  R{Y,  X)Ej  =  fit  ((£,  7)),  (fi,  v))^,  we  have: 

A5d2(^Sd2  -  ^fi1)  +  -  ^fi3) 

dp1^2  -  £2^1)  +  cn3(t2n3  -  z3n2) 

E&i^n1  -  o3^1)  +  Fn2{t2n3  -  £3p2) 

03x1 

Finally,  we  compute  the  —  C*(f))p2  term  that  appears  in  the  equation  for 

Tlihuls  +  l 3^2)77  V 


and 


R((€,v),(£l,v))(n,v)  = 


(15) 


(u*(f)-C*(f))P2  = 


h(hu\z  +  hul^rfri3 


(16) 


Ti{h^\2  +  h^hWp1 

03x1 

If  we  define  the  vectors  (£,  7)1)  and  (77,  v2)  via  the  identification  (£,  v\)(t)  =  E~1pi(t)  and  (77,  7)2)  (i)  = 
£_1P2(i)  then,  we  can  write  the  necessary  conditions  in  terms  of  (■ Q ,  b ,  fi,  v,  £,  7)1,  77, 7)2).  The  full  set 
of  equations  are  (10)  -  (13)  along  with  u  =  (77,7)2)  and: 


+  R((£,7)),(fi,7;))(fi,7;)  -  (w*(/)  -  C*(/))p2  (17) 


£ 

>;  ?rfiWEk 

=  — 

ij,k=l 

Vi 

—V\  X  fi 

V 

>:  v'rfiWEk 

=  — 

i,jtk=l 

V2 

—7)2  x  fi 

77 

1)2 


(18) 


with  (Q,  fi,  b,  77) (0)  and  (Q,£l,b,v)(l)  specified. 


4  Numerical  Experiments 

Equations  ( 10)-(  13) ,  (17)  -  18)  along  with  u  =  (77,7)2)  constitute  a  two-point  boundary  value 
problem.  We  used  a  modified  shooting  method  technique  [7]  to  numerically  solve  for  the  unknown 
’’Lagrange  multipliers”  (£,7)1,77,7)2)  at  initial  time.  The  first  equation  (10)  is  a  matrix  equation 
that  we  integrated  using  the  well-known  Rodriguez’s  formula  [17]: 

f  Cl  Cl2  \ 

Q(t  +  h)  =  Q(t)  (  /  +  -p|j-sin(||P||/i)  +  pp (1  -  cos(||fi||/i))  j  ,  (19) 
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where  h  is  the  time-step  for  integration.  This  results  in  a  Q  matrix  that  ” stays”  on  the  group 
50(3)  at  each  time-step. 

The  moments  of  inertia  constants  for  the  numerical  simulation  where  I\  =  10;  I2  =  7.5;  I3  =  5. 
The  initial  time  to  was  set  to  0  and  the  final  time  tf  was  set  to  1. 

We  chose  the  initial  states  ( Q ,  H,  b,  u)(0)  and  the  desired  final  states  ( Q ,  H,  6,  u)(l)  using  the  MAT- 
LAB  random  number  generator.  Their  values  for  a  simulation  run  are  listed  in  the  following  table 
(to  2  significant  digits). 


Q(0) 

n(o) 

6(0) 

u(0) 

0.572  0.817  0.079 

1.160 

0.950 

0.336 

-0.783  0.572  -0.246 

0.781 

4.337 

1.922 

-0.246  0.079  0.966 

3.693 

7.092 

4.714 

Qdes(l) 

^des(l) 

bdes ( 1 ) 

Vdes  ( 1 ) 

-0.268  0.963  -0.011 

2.319 

1.901 

0.673 

-0.838  -0.227  0.497 

1.562 

8.673 

3.843 

0.476  0.142  0.868 

7.385 

4.185 

9.427 

The  modified  simple  shooting  method  involves  the  choice  of  a  continuous,  time-parametrized  ref¬ 
erence  path  that  connects  the  initial  and  final  points.  For  t  G  [0,1],  we  picked  the  reference 
path  to  be:  Qref(t)  =  Q(0)  exp(</>t),  where  cj)  =  ln(Q(0)_1(5d),  Vtref(t)  =  14(0)  +  —  14(0 ))t, 

bref{t)  =  6(0)  +  (bd  -  b(0))t,  vref(t)  =  u(0)  +  (vd  -  v{0 ))t. 

The  equations  were  integrated  in  the  forward  direction  until  at  some  time  t  G  (0, 1],  we  had 

100  *  ||ln(Q(f))  -  ln(Qref(t))\\  +  ||f2(t)  -  Qref(t) II  +  \\b{t)  -  bref(t)\\  +  \\v(t)  -  vref(t) ||  >  60.  (20) 

At  this  point,  the  initial  guesses  on  the  Lagrange  multipliers  were  updated  via  the  modified  Newton’s 
method  until  the  sum  on  the  left  hand  side  is  less  than  or  equal  to  0.5.  Then  we  shoot  forward 
again  until  the  inequality  in  (20)  was  satisfied  and  the  iteration  was  repeated.  The  norm  on  the 
orientation  matrix  was  multiplied  by  100  so  that  the  final  orientation  is  met  accurately.  The  time 
step  for  the  integration  was  0.02  seconds.  The  CPU  time  taken  for  the  compilations  run  in  a 
MATLAB  environment  running  on  a  1.8  GHz  PC  was  73  seconds.  The  solution  of  the  two-point 
boundary  value  problem  led  to  the  following  final  states  at  t  =  1: 
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Q{  1) 

m 

6(1) 

u(l) 

-0.268  0.963  -0.011 

2.319 

1.901 

0.672 

-0.838  -0.227  0.497 

1.562 

8.674 

3.843 

0.476  0.142  0.868 

7.384 

4.185 

9.426 

The  initial  value  for  the  co-states  (£,  v\ ,  77,  U2)(0)  was  chosen  at  random  using  the  MATLAB  random 
number  generator  (shown  in  the  table  in  the  left  below)  and  converged  to  the  table  on  the  right 
below. 


£(o) 

ui(0) 

7(0) 

u(0) 

m 

ui(0) 

T](0) 

u(0) 

0.145 

0.432 

0.528 

0.336 

-12.839 

-8.620 

-6.766 

-7.527 

0.718 

0.446 

0.573 

0.173 

-18.404 

-8.179 

-9.251 

-0.445 

0.662 

0.508 

0.361 

0.086 

-80.533 

-122.807 

-36.136 

-58.068 

Figures  1(a)  -  1(d)  show  the  results  of  the  simulation.  The  ZYX  Euler  angles  in  Figure  1(a) 
was  computed  according  to:  f3  =  -  sin-1^!);  a  =  sin-1  ;  7  =  sin-1  (e^j)  • 


5  Conclusion 

In  this  paper,  we  have  made  three  new  contributions.  Firstly,  we  have  derived  first  order  necessary 
conditions  for  an  optimal  control  problem  on  a  parallelizable  Riemannian  manifold,  using  frame  co¬ 
ordinates.  These  equations  specialize  to  those  of  cubic  splines  on  Riemannian  manifolds  that  were 
first  discovered  by  Noakes,  Heinzinger  and  Paden.  Secondly,  we  have  specialized  the  equations  to 
a  rigid  body  translation  and  rotation  problem.  Thirdly,  we  have  presented  the  results  of  numerical 
experiments  where  we  successfully  computed  the  two  point  boundary  value  problem  (TPBVP) 
resulting  from  the  necessary  conditions. 
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